/*
 * JRoverSensors.cpp
 *
 *  Created on: Nov 22, 2012
 *      Author: bouchier
 */
/*! \file JRoverSensors.cpp
 * \brief This file defines the JRoverSensors class
 */
#include <cmath>
#include <CQEI2C.h>
#include <qegpioint.h>
#include <qeanalog.h>
#include <RCTest.h>
#include <usonicRanger.h>
#include <ControlledMotor.h>
#include "JRoverSensors.h"

#define METERS_TO_INCHES 39.37

// Robot physical parameters in inches
#define WIDTH 18
#define PI 3.14159265359
#define TWOPI 2 * PI

extern CQEGpioInt &gpio;
extern CQEAnalog &analog;
extern CQEI2C i2c;
extern ControlledMotor lmDrive;
extern ControlledMotor rmDrive;

JRoverSensors::JRoverSensors() {
	// initialize class variables
	X = Y = theta = 0.0;
	lmDistance = lastLMDistance = rmDistance = lastRMDistance = 0.0;
}

JRoverSensors::~JRoverSensors() {
}

//! Initialize sensor software and set up for polling sensors
/*!
 * Initialize the R/C channel structs and start polling R/C channels at interrupt level.
 * Initialize the ultrasonic ranger structs and start measuring distance at interrupt level
 * using a state machine which sequences through the 3 range sensors each call to pollSensors()
 */
void JRoverSensors::initSensors() {
	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
	dirRcc = initRCTest(gpio, 1);		// initialize RC channel on DIO 0
	speedRcc = initRCTest(gpio, 2);
	estopRcc = initRCTest(gpio, 3);

	// ACHTUNG - CHECK THE DIO NUMBERS
	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
	lRanger = initUsonicRanger(gpio, 12, 11);		// initialize left usonic ranger on DIO 12 & 13
	cRanger = initUsonicRanger(gpio, 14, 13);
	rRanger = initUsonicRanger(gpio, 16, 15);
	currentUsonicRanger = 0;
}

//! Check the next sensor that needs to have measurement kicked off
void JRoverSensors::pollSensors()
{
	// trigger the next ultrasonic range sensor
	switch (currentUsonicRanger) {
	case 0:
		triggerUsonicRanger(cRanger); currentUsonicRanger++; break;
	case 1:
		triggerUsonicRanger(rRanger); currentUsonicRanger++; break;
	case 2:
		triggerUsonicRanger(lRanger); currentUsonicRanger=0;
		//printf("Ranges: L: %d, C: %d, R: %d\n", lRanger->distance, cRanger->distance, rRanger->distance);
		break;
	}

	// get odometry & compute new pose
	updateOdometers();

	// read battery voltage
	voltage = (float)analog.GetADVoltage(24);
	voltage /= 1000.0;

	// copy interesting data into the ROS msg. Note: ROSIF fills in rosLinear & rosAngular
	int i = 2;
	robotTelemetry[i++] = (float)getRcSpeed();
	robotTelemetry[i++] = (float)getRcDir();
	robotTelemetry[i++] = (float)getRcEstop();
	robotTelemetry[i++] = X;
	robotTelemetry[i++] = Y;
	robotTelemetry[i++] = theta;
	robotTelemetry[i++] = voltage;
	robotTelemetry[i++] = getLDistance();
	robotTelemetry[i++] = getCDistance();
	robotTelemetry[i++] = getRDistance();
}

//! Update current position based on odometers
/*!
 * Read the delta left & right distance and update estimated X, Y, theta pose based on odometry
 */
void JRoverSensors::updateOdometers()
{
	float deltaLDistance, deltaRDistance, deltaDistance, deltaTheta;

	// calculate how far robot has traveled since last sample
	lastLMDistance = lmDistance;
	lastRMDistance = rmDistance;
	lmDistance = lmDrive.getDistance();
	rmDistance = rmDrive.getDistance();
	deltaLDistance = lmDistance - lastLMDistance;
	deltaRDistance = rmDistance - lastRMDistance;

    /* calculate distance we have traveled since last sampling */
    deltaDistance = (deltaLDistance + deltaRDistance) / 2.0;

    /* accumulate total rotation around our center */
    deltaTheta = (deltaLDistance - deltaRDistance) / WIDTH;
    theta += deltaTheta;

    /* and clip the rotation to plus or minus 360 degrees */
    theta -= (float)((int)(theta/TWOPI))*TWOPI;

    /* now calculate and accumulate our position in inches */
    Y += deltaDistance * cos(theta);
    X += deltaDistance * sin(theta);
}

//! Get pulsewidth from speed channel
int JRoverSensors::getRcSpeed()
{
	return(speedRcc->pulseWidth);
}

//! Get pulsewidth from direction channel
int JRoverSensors::getRcDir()
{
	return(dirRcc->pulseWidth);
}

//! Get pulsewidth from estop channel
int JRoverSensors::getRcEstop()
{
	return(estopRcc->pulseWidth);
}

//! Get main battery voltage
/*
 * \return Voltage in volts of main battery
 */
float JRoverSensors::getBatteryVoltage()
{
	return voltage;
}

//! Get reft ultrasonic ranger distance
int JRoverSensors::getLDistance()
{
	return lRanger->distance;
}

//! Get center ultrasonic ranger distance
int JRoverSensors::getCDistance()
{
	return cRanger->distance;
}

//! Get right ultrasonic ranger distance
int JRoverSensors::getRDistance()
{
	return rRanger->distance;
}
